#include <ros/ros.h>
#include "kinematic.h"
#include <ubt_core_msgs/JointCommand.h>
#include "webots_api/SceneSelection.h"
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Twist.h>
#include "walker_srvs/leg_motion_MetaFuncCtrl.h"
#include <std_msgs/String.h>

geometry_msgs::Point center_data;
std::string leg_status;
void subCenterData(const geometry_msgs::Point &msgs) {
    center_data = msgs;
}

void subWalkerStatus(const std_msgs::String &msgs) {
    leg_status = msgs.data;
}

int main(int argc, char **argv) {
    Matrix_Init();
    ros::init(argc, argv, "PushCart_node");
    ros::NodeHandle nh;

    ros::ServiceClient client = nh.serviceClient<webots_api::SceneSelection>("/walker/sence");
    ros::ServiceClient walk_client = nh.serviceClient<walker_srvs::leg_motion_MetaFuncCtrl>("/Leg/TaskScheduler");

    ros::Subscriber center_data_sub = nh.subscribe("/Leg/dis_waist2ankle_left", 10, &subCenterData);
    ros::Subscriber walker_status_sub = nh.subscribe("/Leg/leg_status", 10, &subWalkerStatus);
    
    ros::Publisher walker_vel = nh.advertise<geometry_msgs::Twist>("/nav/cmd_vel_nav", 10);
    geometry_msgs::Twist vel_ctrl;

    walker_srvs::leg_motion_MetaFuncCtrl walker_walking;
    walker_walking.request.func_name = "dynamic";
    walker_walking.request.cmd = "start";
    
    webots_api::SceneSelection scene_selection;
    scene_selection.request.scene_name = "PushCart";
    scene_selection.request.nav = false;
    scene_selection.request.vision = false;
    
    std::cout << "Wait for SceneSelect Service" << std::endl;
    while(!client.call(scene_selection))
    {
        if(!ros::ok())
        {
            return 0;
        }
    }
    ROS_INFO("Scene select success!");

    ros::Publisher left_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftLimb/controller", 10);
    ubt_core_msgs::JointCommand left_arm_data;
    left_arm_data.command.resize(7);
    left_arm_data.mode = 5;

    ros::Publisher left_finger_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/leftHand/controller", 10);
    ubt_core_msgs::JointCommand left_finger_data;
    left_finger_data.command.resize(10);
    left_finger_data.mode = 5;

    ros::Publisher right_arm_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/rightLimb/controller", 10);
    ubt_core_msgs::JointCommand right_arm_data;
    right_arm_data.command.resize(7);
    right_arm_data.mode = 5;

    ros::Publisher right_finger_pub = nh.advertise<ubt_core_msgs::JointCommand>("/walker/rightHand/controller", 10);
    ubt_core_msgs::JointCommand right_finger_data;
    right_finger_data.command.resize(10);
    right_finger_data.mode = 5;

    Eigen::VectorXd left_joints(7);
    left_joints << 0,0,0,0,0,0,0;
    Eigen::VectorXd right_joints(7);
    right_joints << 0,0,0,0,0,0,0;

    Eigen::Vector3d position, orientation;

    ros::Rate loop_rate(1000);
    double time_count = 0.0;
    float x_bias;
    while(ros::ok())
    {
        time_count += 0.001;
        if(leg_status == "standing")
        {
            walk_client.call(walker_walking);
        }
        if(leg_status == "dynamic")
        {
            x_bias = (center_data.y - 0.0798)*1000;
        }
        else
        {
            x_bias = 0;
        }
        if(time_count < 1)
        {
            position << 400-100*(time_count)+x_bias,275*(time_count),400-250*(time_count);
            if(leftArmInverse(position, orientation, left_joints, false) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -400+100*(time_count)+x_bias,275*(time_count),400-250*(time_count);
            if(rightArmInverse(position, orientation, right_joints, false) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        else if(time_count < 1.5)
        {
            position << 300-200*(time_count-1)+x_bias, 275+100*(time_count-1), 150+80*(time_count-1);
            orientation << M_PI/4,M_PI/2,-M_PI/2;
            if(leftArmInverse(position, orientation, left_joints, true) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -300+200*(time_count-1)+x_bias, 275+100*(time_count-1), 150+80*(time_count-1);
            orientation << -M_PI/4,M_PI/2,M_PI/2;
            if(rightArmInverse(position, orientation, right_joints, true) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }
        }
        else if(time_count < 1.55)
        {
            position << 200+x_bias, 325, 190;
            orientation << M_PI/4,M_PI/2,-M_PI/2;
            if(leftArmInverse(position, orientation, left_joints, true) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -200+x_bias, 325, 190;
            orientation << -M_PI/4,M_PI/2,M_PI/2;
            if(rightArmInverse(position, orientation, right_joints, true) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }

            float angle = 1.5;
            left_finger_data.command[0] = angle;
            left_finger_data.command[1] = angle;
            left_finger_data.command[2] = angle;
            left_finger_data.command[3] = angle;
            left_finger_data.command[4] = angle;
            left_finger_data.command[5] = angle;
            left_finger_data.command[6] = angle;
            left_finger_data.command[7] = angle;
            left_finger_data.command[8] = angle;
            left_finger_data.command[9] = angle;
            left_finger_pub.publish(left_finger_data);

            right_finger_data.command[0] = angle;
            right_finger_data.command[1] = angle;
            right_finger_data.command[2] = angle;
            right_finger_data.command[3] = angle;
            right_finger_data.command[4] = angle;
            right_finger_data.command[5] = angle;
            right_finger_data.command[6] = angle;
            right_finger_data.command[7] = angle;
            right_finger_data.command[8] = angle;
            right_finger_data.command[9] = angle;
            right_finger_pub.publish(right_finger_data);
        }
        else
        {
            position << 200+x_bias, 325, 190;
            orientation << M_PI/4,M_PI/2,-M_PI/2;
            if(leftArmInverse(position, orientation, left_joints, true) == true)
            {
                left_arm_data.command[0] = left_joints(0);
                left_arm_data.command[1] = left_joints(1);
                left_arm_data.command[2] = left_joints(2);
                left_arm_data.command[3] = left_joints(3);
                left_arm_data.command[4] = left_joints(4);
                left_arm_data.command[5] = left_joints(5);
                left_arm_data.command[6] = left_joints(6);
                left_arm_pub.publish(left_arm_data);
            }

            position << -200+x_bias, 325, 190;
            orientation << -M_PI/4,M_PI/2,M_PI/2;
            if(rightArmInverse(position, orientation, right_joints, true) == true)
            {
                right_arm_data.command[0] = right_joints(0);
                right_arm_data.command[1] = right_joints(1);
                right_arm_data.command[2] = right_joints(2);
                right_arm_data.command[3] = right_joints(3);
                right_arm_data.command[4] = right_joints(4);
                right_arm_data.command[5] = right_joints(5);
                right_arm_data.command[6] = right_joints(6);
                right_arm_pub.publish(right_arm_data);
            }

            if(leg_status == "dynamic")
            {
                vel_ctrl.linear.x = 0.2;
                walker_vel.publish(vel_ctrl);
            }
        }
        
        ros::spinOnce();
        loop_rate.sleep();
    }
    return 0;
}